/*
 * Application layer (MIN layer 2)
 *
 * Application-specific handling of data to and from the host.
 *
 * Author: Ken Tindell
 * Copyright (c) 2014-2015 JK Energy Ltd.
 * Licensed under MIT License.
 */

#include <iostream>
#include <vector>
#include <queue>
#include <fstream>
#include <stdio.h>
#include <string>

#include "conf.h"
#include "min.h"
#include "layer2.h"
#include "ros/ros.h"
#include "navigation_msg/odom.h"

#define  MIN_ZDEPTH_OBTACLE 5
using namespace std;

small_car_pulse Small_Car_Pulse;
extern int start_status;
extern int stop_status;

int step_turn_pulse=0;
int8_t sensor_massage_mark=0;
int8_t ladar_massage_mark=0;
int8_t is_hitting=0;

queue<TargetStatusTypeDef> queue_radar;
TargetStatusTypeDef TargetStatus_t;
TargetStatusTypeDef TargetStatusRes_t;

/* Configure the UART speed (see serial.h for binding to UART and FIFO buffer sizes) */
#define MIN_BAUD                        (UART_BAUD_115200)

/* Worker functions to write words into a buffer in big endian format
 * using generic C
 *
 * A good C compiler will spot what is going on and use byte manipulation
 * instructions to get the right effect (functions declared static
 * to hint to compiler that code can be inlined)
 */
static void encode_32(uint32_t data, uint8_t buf[])
{
    buf[0] = (uint8_t)((data & 0xff000000UL) >> 24);
    buf[1] = (uint8_t)((data & 0x00ff0000UL) >> 16);
    buf[2] = (uint8_t)((data & 0x0000ff00UL) >> 8);
    buf[3] = (uint8_t)(data & 0x000000ffUL);
}

static void encode_16(uint32_t data, uint8_t buf[])
{
    buf[0] = (uint8_t)((data & 0x0000ff00UL) >> 8);
    buf[1] = (uint8_t)(data & 0x000000ffUL);
}

static uint32_t decode_32(uint8_t buf[])
{
    uint32_t res;

    res = ((uint32_t)(buf[0]) << 24) | ((uint32_t)(buf[1]) << 16) | ((uint32_t)(buf[2]) << 8) | (uint32_t)(buf[3]);

    return res;
}

static uint16_t decode_16(uint8_t buf[])
{
    uint16_t res;
    res = ((uint16_t)(buf[0]) << 8) | (uint16_t)(buf[1]);
    return res;
}



/* Macros for unpacking and packing MIN frames in various functions.
 *
 * Declaring stuff in macros like this is not pretty but it cuts down
 * on obscure errors due to typos.
 */
#define DECLARE_BUF(size)		uint8_t m_buf[(size)]; uint8_t m_control = (size); uint8_t m_cursor = 0
#define PACK8(v)				((m_cursor < m_control) ? m_buf[m_cursor] = (v), m_cursor++ : 0)
#define PACK16(v)				((m_cursor + 2U <= m_control) ? encode_16((v), m_buf + m_cursor), m_cursor += 2U : 0)
#define PACK32(v)				((m_cursor + 4U <= m_control) ? encode_32((v), m_buf + m_cursor), m_cursor += 4U : 0)
#define SEND_FRAME(id)			(min_tx_frame((id), m_buf, m_control))

#define DECLARE_UNPACK()		uint8_t m_cursor = 0
#define UNPACK8(v)				((m_cursor < m_control) ? ((v) = m_buf[m_cursor]), m_cursor++ : ((v) = 0))
#define UNPACK16(v)				((m_cursor + 2U <= m_control) ? ((v) = decode_16(m_buf + m_cursor)), m_cursor += 2U : ((v) = 0))
#define UNPACK32(v)				((m_cursor + 4U <= m_control) ? ((v) = decode_32(m_buf + m_cursor)), m_cursor += 4U : ((v) = 0))


/* Functions called by the application to report information via MIN frames */
static void update_sensor(uint8_t m_id, uint8_t m_buf[], uint8_t m_control)
{

    DECLARE_UNPACK();

    int32_t imu_pitch ;
    int32_t imu_roll;
    int32_t imu_yaw;

    UNPACK32(Small_Car_Pulse.left_speed);
    UNPACK32(Small_Car_Pulse.right_speed);
    UNPACK32(Small_Car_Pulse.car_angle);
    
   UNPACK16(Small_Car_Pulse.motor_pulse);

   printf("small_car_speed:%d\r\n",Small_Car_Pulse.left_speed);
    /*UNPACK32(imu_pitch);
    UNPACK32(imu_roll);
    UNPACK32(imu_yaw);

    Small_Car_Pulse.imu_pitch=(float)(imu_pitch/10);
    Small_Car_Pulse.imu_roll=(float)(imu_roll/10);
    Small_Car_Pulse.imu_yaw=(float)(imu_yaw)/10;*/

    if((abs(Small_Car_Pulse.left_speed)>1)&&(Small_Car_Pulse.left_speed<1200)){

         ros::param::set("turn_enable",1);
         if(Small_Car_Pulse.car_angle>=-2){
              ros::param::set("turn_left_enable",0);

         }else if(Small_Car_Pulse.car_angle<=-100){
              ros::param::set("turn_right_enable",0);
         }else{
              ros::param::set("turn_right_enable",1);
              ros::param::set("turn_left_enable",1);
         }

    }else{
         ros::param::set("turn_enable",0);
    }

    sensor_massage_mark=1;

}

void update_radar(uint8_t m_id, uint8_t m_buf[], uint8_t m_control)
{
    uint8_t Data[8]={0};
    uint32_t FreamID=0;
    static  uint8_t  index_count=0;
    DECLARE_UNPACK();

    UNPACK32(FreamID);
    for(uint8_t i=0;i<8;i++)
    UNPACK8(Data[i]);

    ladar_massage_mark=1;

    switch(FreamID)
    {
        case RadarFeedback:


        case TargetStatus :

            if(TargetStatus_t.nextstep==RecTargetStatus)
            {
                TargetStatus_t.NumOfCluster=Data[0];
                TargetStatus_t.Radar_RollCount=Data[1]&0x03;
                TargetStatus_t.nextstep=RecInTargetInfo;
                //TargetStatus_t.TargeInfo[8]={{},{},{},{},{},{},{},{}};
                memset(TargetStatus_t.TargeInfo,0,sizeof(TargetStatus_t.TargeInfo[1])*8);
               //  printf("NumOfCluster=%d,nextstep=%d\r\n"   ,TargetStatus_t.NumOfCluster,TargetStatus_t.nextstep);


            }
        #if 0
            printf("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\r\n");
            printf("NumOfCluster=%d\r\n"   ,TargetStatus_t.NumOfCluster);
            printf("Radar_RollCount=%d\r\n",TargetStatus_t.Radar_RollCount);
            //printf("nextstep=%d\r\n",TargetStatus_t[Candevice_ID].nextstep);
            printf("\r\n");
        #endif
        break;

        case TargerInformation :
                 //  printf("nextstep=%d\r\n"   ,TargetStatus_t.nextstep);
                //	printf("###############################################################\r\n");
                if(TargetStatus_t.nextstep==RecInTargetInfo)
                {

                    TargetStatus_t.TargeInfo[index_count].Cluster_Index=Data[0];
                    TargetStatus_t.TargeInfo[index_count].Cluster_RCSValue=Data[1]*0.5-50;
                    TargetStatus_t.TargeInfo[index_count].Cluster_Range=((Data[2]<<8)|Data[3])*0.01;
                    TargetStatus_t.TargeInfo[index_count].Cluster_Azimuth=Data[4]-90;
                    TargetStatus_t.TargeInfo[index_count].Radar_RollCount=Data[5]>>6;
                    TargetStatus_t.TargeInfo[index_count].Cluster_Vrel=(((Data[5]&0x07)<<8)|Data[6])*0.05-35;
                    TargetStatus_t.TargeInfo[index_count].Cluster_SNR =Data[7]-127;
                    index_count++;
                    //printf("Cluster_Index=%d,index=%d\r\n"   ,TargetStatus_t.NumOfCluster,index_count);
                    if(TargetStatus_t.NumOfCluster==(index_count-1))
                    {
                        //printf("push !!!!!!!!!!!!!!! \r\n");
                        TargetStatus_t.nextstep=TargetDetectOver;
                        queue_radar.push(TargetStatus_t);
                        TargetStatus_t.nextstep=RecTargetStatus;
                        index_count=0;

                    }

                }
                #if 0
                printf("###############################################################\r\n");
                printf("Cluster_Index=%d\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_Index);
                printf("Cluster_RCSValue=%f\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_RCSValue);
                printf("Cluster_Range=%f\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_Range);
                printf("Cluster_Azimuth=%d\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_Azimuth);
                printf("Radar_RollCount=%d\r\n",TargetStatus_t.TargeInfo[index_count].Radar_RollCount);
                printf("Cluster_Vrel=%f\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_Vrel);
                printf("Cluster_SNR=%d\r\n",TargetStatus_t.TargeInfo[index_count].Cluster_SNR);
                printf("\r\n");
                #endif

        break;

        default:
            break;

    }

}



void move_cmd(int16_t speed_enable,int16_t target_speed,int16_t dir_enable,int16_t target_angle,int16_t  target_dir)
{
    DECLARE_BUF(10);
    PACK16(speed_enable);
    PACK16(target_speed);
    PACK16(dir_enable);
    PACK16(target_angle);
    PACK16(target_dir);

    SEND_FRAME(MIN_ID_MOVE_CMD);
}

void set_motor_duty(unsigned char motor, unsigned char dir, unsigned short duty)
{
    DECLARE_BUF(4);

    PACK8(motor);
    PACK8(dir);

    PACK16(duty);

    SEND_FRAME(MIN_ID_SET_DUTY);
}


/* Functions to unpack incoming MIN frames into application-specific data.
 */
int int_SerialFD = -1;

/* Other application-specific frame-decode functions would be added here.
 */

#define MAX_SIZE 1024

/* Main function to process incoming bytes and pass them into MIN layer */
void poll_rx_bytes(void)
{
    uint8_t buffer[MAX_SIZE];
    int pos = 0, last_pos = 0, search = 0;
    int i ;

    pos = read(int_SerialFD, &buffer[last_pos], MAX_SIZE);

    if (pos < 0)
    {
        //An error occured (will occur if there are no bytes)
        sleep(1);
        return;
    }
    else if (pos == 0)
    {
        //No data waiting
        usleep(10);
        return;
    }
    //Bytes received

    /* Handle all the outstanding characters in the input buffer */
    for(i = 0; i < pos; i++){
        min_rx_byte(buffer[i]);
    }
}


/* Callback from MIN layer 1 to indicate the frame has been received
 */
void min_frame_received(uint8_t buf[], uint8_t control, uint8_t id)
{
    //printf("id is  %x\n", id);
    switch(id) {

        case MIN_ID_ENVIRONMENT:
            update_sensor(id, buf, control);
            break;
        case  MIN_ID_RADAR:
               update_radar(id, buf, control);
               break;
        default:
            break;
        /* Other IDs would be handled in this case statement, calling the
         * application-specific functions to unpack the frames into
         * application data for passing to the rest of the application
         */
    }
}





#define MAX_TX_BUFFER	1024

static uint8_t tx_buffer[MAX_TX_BUFFER];
static uint8_t tx_ptr = 0;

void min_tx_temp_store(uint8_t byte)
{
    if(tx_ptr < MAX_TX_BUFFER){
        tx_buffer[tx_ptr] = byte;
        tx_ptr++;
    }
}

/* Callback from MIN to send a byte over the UART (in this example, queued in a FIFO) */
void min_tx(void)
{
    int n = 0;
    int tmp = 0;
    n = write(int_SerialFD, tx_buffer,tx_ptr);
    tx_ptr = 0;
}

/* Callback from MIN to see how much transmit buffer space there is */
// uint8_t min_tx_space(void)
// {
    // return uart_send_space();
// }

///////////////////////////////////////////
string   SERIAL_PORT="/dev/ttyUSB0";
#define BAUDRATE 			B115200 //


int init_uart(string uart_port, int & device_id)
{

    struct termios OldTerm, NewTerm;
    int int_Flag = 0;
    const char* port=uart_port.c_str();
    device_id = open(port, O_RDWR|O_NOCTTY);//
    printf("open %s... \n", port);
    printf("com is %d  \n", device_id);
    if (device_id == -1)
    {
        printf("\n\rOpen serial port %s error \n",port);
        perror("Error reas on");
        return -1;
    }

    if(tcgetattr(device_id,&OldTerm) < 0)
    {
        printf("tc get attribute Error! \n");
        return -1;
    }
    bzero(&NewTerm,sizeof(NewTerm));
    NewTerm = OldTerm;
    fcntl(device_id,F_SETFL,0);
    NewTerm.c_cflag |= CLOCAL|CREAD;
    NewTerm.c_cflag &= ~PARENB;
    NewTerm.c_cflag &= ~CSTOPB;
    NewTerm.c_cflag &= ~CSIZE;
    NewTerm.c_cflag &= ~CRTSCTS;
    NewTerm.c_cflag |= CS8;
    NewTerm.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    NewTerm.c_oflag &= ~OPOST;
    NewTerm.c_cc[VMIN] = 20;
    NewTerm.c_cc[VTIME] = 5;
    // NewTerm.c_iflag &=~(IXON | IXOFF | IXANY | INPCK);
    NewTerm.c_iflag &=~(IXON | IXOFF | IXANY );

    tcflush(device_id,TCIFLUSH);


    cfsetispeed(&NewTerm,BAUDRATE);
    cfsetospeed(&NewTerm,BAUDRATE);
    printf("\n\r COM is [%d]\n\r", device_id);
    int_Flag = tcsetattr(device_id,TCSANOW,&NewTerm);
    if(int_Flag != 0)
    {
        printf("set attribute error !\n");
        return -1;
    }
    printf("\n\rOpened...\n");

    return 0;
}


void init_min(void)
{
    /* Set MIN Layer 0 settings of 8 data bits, 1 stop bit, no parity */
    init_uart(SERIAL_PORT,int_SerialFD );
    min_init_layer1();

}

void motion(int16_t speed_enable,int16_t target_speed,int16_t dir_enable,int16_t target_angle,int16_t  target_dir){

   move_cmd(speed_enable,target_speed,dir_enable,target_angle,target_dir);

}



void* pthread_uart_monitor(void*)
{
    init_min();
    if(int_SerialFD < 0){
        pthread_exit(NULL);
    }
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);

    while(1){
        poll_rx_bytes();
        usleep(5000);
    }

    if(int_SerialFD > 0)
        close(int_SerialFD);
    pthread_exit(NULL);
}


void * pthread_ladar(void *){

    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
    while(1){
        get_ladar_distance();
        usleep(20000);
    }
    pthread_exit(NULL);
}


void get_ladar_distance(){

  if(!queue_radar.empty())
    {
        TargetStatusRes_t=queue_radar.front();

        for(int i=0;i<min(10,(int)TargetStatusRes_t.NumOfCluster);i++)
        {
            /*printf("###############################################################\r\n");
            printf("Cluster_Index=%d\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_Index);
            printf("Cluster_RCSValue=%f\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_RCSValue);
            printf("Cluster_Range=%f\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_Range);
            printf("Cluster_Azimuth=%d\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_Azimuth);
            printf("Radar_RollCount=%d\r\n",TargetStatusRes_t.TargeInfo[i].Radar_RollCount);
            printf("Cluster_Vrel=%f\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_Vrel);
            printf("Cluster_SNR=%d\r\n",TargetStatusRes_t.TargeInfo[i].Cluster_SNR);
            printf("\r\n");*/


            double theta=TargetStatusRes_t.TargeInfo[i].Cluster_Azimuth*3.1415/180;
            double dx=(TargetStatusRes_t.TargeInfo[i].Cluster_Range*sin(theta));
            double dz=(TargetStatusRes_t.TargeInfo[i].Cluster_Range*cos(theta));
            double p3[3]={dx,0,dz};

           // int u=int((kmat[0]*dx)/dz+kmat[2]+0.5);
           // printf("Position: [%03d %03d]\n",u,width);
            //cv::Scalar pencolor=Scalar(color_blue[i],color_green[i],color_red[i]);
            //cv::circle(anaglyph_left[0],cv::Point(u,v),int(height*0.05),pencolor,2,8);
                //Point center;
                //center.y=v;
                //center.x=u;
                //circle(img,center,img.rows/20,Scalar(0, 0, 255),5,8);
                //Point center_radius;
            //int object_radius=int((kmat[0]*size_object)/dz+0.5);
                //center_radius.y=object_radius;
                //center_radius.x=object_radius/2;
               // rectangle(anaglyph_left[0], center-center_radius,center+center_radius,pencolor,3,8);

           // if(abs(dx)<1.5&&abs(dz)<min_zdepth_obstacle)//about to hit the obstacle!!!!
            if(abs(dx)<1&&abs(dz)<MIN_ZDEPTH_OBTACLE)//about to hit the obstacle!!!!
            {
                is_hitting=1;
                break;

            }else{

                is_hitting=0;
            }

        }
        //if(is_hitting)
        //{
           // drawText(anaglyph_left[0]);
        //}
        queue_radar.pop();
    }

}




